clear,clc

l=22100;
x=1:l;
h=zeros(l,1);   %设置地图海拔
h(1:1900)=linspace(591,560,1900);
h(1901:2700)=linspace(560,493,800);
h(2701:4900)=linspace(493,455,2200)
h(4901:10300)=linspace(455,676,5400);
h(10301:15500)=linspace(676,500,5200);
h(15501:17500)=linspace(500,590,2000);
h(17501:22100)=linspace(590,591,4600);  

GR=zeros(l,1);
GR=[0;h(2:end)-h(1:end-1)];
% GR=atan(GR);
figure()
plot(x,h)
hold on
figure()
plot(GR)


CP=500;
Va=3;  %风速
mT=90; %总质量

% v=-20:.1:20;
alpha=1/2*1.2234*(0.2565+0.0044);
beta=(0.0032*mT*9.80665);
% p=alpha*v.*(v+Va).^2+beta*v;
% figure()
% plot(v,p)
% clf
v=11*exp(0);  %设置车手速度Vg
% GR=(-1/sqrt(3)):(1/sqrt(3)/100):(1/sqrt(3)); %设置梯度角范围
% for i=1:1
%     p=alpha*v*(v+Va)^2+beta*v*cos(atan(GR))+v*mT*9.80665*sin(atan(GR));
%     line([0 0],[min(p) max(p)],'Color','k','LineWidth',1)
%     hold on
%     line([min(GR) max(GR)],[0 0],'Color','k','LineWidth',1)
%     plot(GR,p,'LineWidth',2)
%     grid on,set(gca,'GridLineStyle','--')
%     xlabel('$G_{R}\ \ unit: 1$','Interpreter','latex')
%     set(gca,'Xtick',[-1/sqrt(3) 1/sqrt(3)])
%     set(gca,'XtickLabel',{'-tan30°','tan30°'})
%     title('$p\ \ unit: watt\ \ \ \ v_{a}=3m/s,v=11m/s$','Interpreter','latex')
%     
%     [ans,ind]=find(GR==0);
%     scatter(GR(ind),p(ind),'MarkerEdgeColor',[0 0 0])
%     text(GR(ind),p(ind),['   ' num2str(p(ind))])
% end

p=alpha*v*(v+Va)^2+beta*v*cos(atan(GR))+v*mT*9.80665*sin(atan(GR));
figure()
plot(GR,p)


